#include "/apollo/modules/tools/test/osqp_test/src/common/smoothing/matplotlibcpp.h"
#include "cubic_spline.h"
#include <algorithm>
#include <iostream>
#include <math.h>
#include <memory>
#include <string>
#include <vector>

using namespace cpprobotics;
namespace plt = matplotlibcpp;

// 定义节点
class Node {
public:
  double x;
  double y;
  int index_x;
  int index_y;
  double cost;
  Node *precent_node;
  Node(double x_, double y_, double index_x_, double index_y_, double cost_,
       Node *precent_node_ = nullptr)
      : x(x_), y(y_), index_x(index_x_), index_y(index_y_), cost(cost_),
        precent_node(precent_node_){};
  Node &operator=(Node &node) { return node; };
  ~Node() = default;
};

struct Path {
  std::vector<float> x;
  std::vector<float> y;
  std::vector<float> heading;
  std::vector<float> kappa;
  std::vector<float> s;
};

// 动态规划
class DP {
public:
  // DP网格地图
  std::vector<std::vector<Node>> dp_map_;
  // 初始状态
  double init_x;
  double init_y;
  double init_heading;
  //   网格分辨率大小
  double resolution_x;
  double resolution_y;
  int nx;                  // x方向的网格数量
  int ny;                  // y方向的网格数量
  Path best_path;          // DP最优路径
  Path best_path_smoother; // DP最优路径的平滑结果
  // 各种代价的权重
  double weight_init_error = 50;
  double weight_error = 1;
  double weight_first_dot = 10.0;
  double weight_second_dot = 0.0;
  double weight_third_dot = 0.0;
  double weight_obstale = 0;
  DP(double rx, double ry) : resolution_x(rx), resolution_y(ry){};
  ~DP() = default;

  // 设置初值
  void set_init_state(double x, double y, double heading) {
    init_x = x;
    init_y = y;
    init_heading = heading;
  }

  //   生成网格地图
  void generate_map(double x, double y) {
    nx = x / resolution_x;
    ny = y / resolution_y;

    for (int i = 0; i <= nx; i++) {
      std::vector<Node> temp;
      for (int j = 0; j <= ny; j++) {
        double x = i * resolution_x;
        double y = j * resolution_y;
        temp.push_back(Node(x, y, i, j, 0, nullptr));
      }
      dp_map_.emplace_back(temp);
    }
  }

  //   计算中代价；
  void calc_total_cost() {
    // 赋初值
    int init_index_y = init_y / resolution_y;
    for (int j = 0; j <= ny; j++) {
      dp_map_[1][j].precent_node =
          new Node(init_x, init_y, 0, init_index_y, 0, nullptr);
    }
    for (int i = 1; i <= nx; i++) {
      std::cout << "h" << std::endl;
      for (int j = 0; j <= ny; j++) {
        auto &current_node = dp_map_[i][j];
        // current_node->cost = current_node->precent_node->cost;
        calc_precent_node_and_cost(current_node);
        std::cout << current_node.x << "," << current_node.y << ","
                  << current_node.cost << std::endl;
      }
    }
  };

  // 计算前一个节点
  void calc_precent_node_and_cost(Node &node) {
    int best_index_y = 0;
    double best_cost = std::numeric_limits<double>::infinity();
    int index_x = node.index_x;
    if (index_x == 1) {
      auto precent_node =
          dp_map_[node.precent_node->index_x][node.precent_node->index_y];
      best_cost = calc_cost(node, precent_node);
      best_index_y = node.precent_node->index_y;
    } else {
      for (int j = 0; j <= ny; j++) {
        auto precent_node = dp_map_[index_x - 1][j];
        double cost = calc_cost(node, precent_node);
        if (best_cost > cost) {
          best_cost = cost;
          best_index_y = j;
        }
      }
    }
    node.precent_node = &dp_map_[index_x - 1][best_index_y];
    node.cost = best_cost;
  };

  // 计算移动的代价
  double calc_cost(Node &node, Node &precent_node) {
    double cost = 0.0;
    cost +=
        weight_init_error * std::pow(calc_init_error(node, precent_node), 2);
    cost += weight_error * std::pow(calc_error(node), 2);
    cost += weight_first_dot * std::pow(calc_first_dot(node, precent_node), 2);
    cost +=
        weight_second_dot * std::pow(calc_second_dot(node, precent_node), 2);
    cost += weight_third_dot * std::pow(calc_third_dot(node, precent_node), 2);
    cost += precent_node.cost;
    return cost;
  };

  // 计算初始状态误差,主要针对朝向角
  double calc_init_error(Node &current_node, Node &precent_node) {
    if (current_node.index_x == 1) {
      double heading = std::atan(calc_first_dot(current_node, precent_node));
      return heading - init_heading;
    } else {
      return 0;
    }
  }

  //   计算一阶导代价
  double calc_first_dot(Node &current_node, Node &precent_node) {
    return (current_node.y - precent_node.y) /
           (current_node.x - precent_node.x);
  };

  //   计算二阶导代价
  double calc_second_dot(Node &current_node, Node &precent_node) {
    if (current_node.index_x < 2) {
      return 0.0;
    } else {
      double first_dot = calc_first_dot(current_node, precent_node);
      double first_dot_precent =
          calc_first_dot(precent_node, *precent_node.precent_node);
      return (first_dot - first_dot_precent) /
             ((current_node.x - precent_node.x));
    }
  };

  //   计算三阶导代价
  double calc_third_dot(Node &current_node, Node &precent_node) {
    if (current_node.index_x < 3) {
      return 0.0;
    } else {
      double second_dot = calc_second_dot(current_node, precent_node);
      double second_dot_precent =
          calc_second_dot(precent_node, *precent_node.precent_node);
      return (second_dot - second_dot_precent) /
             ((current_node.x - precent_node.x));
    }
  };

  //   计算与参考路径的误差
  double calc_error(Node &current_node) { return current_node.y; };

  // 碰撞检测
  bool iscollision();

  // 获取最优的结果
  void get_final_path() {
    calc_total_cost();
    // std::pair<double, double> best_path;
    double best_cost = std::numeric_limits<double>::infinity();
    auto node = &dp_map_[nx][0];
    for (int j = 0; j <= ny; j++) {
      if (best_cost > dp_map_[nx][j].cost) {
        node = &dp_map_[nx][j];
        best_cost = node->cost;
      }
    }
    std::cout << "---------------best path--------------" << std::endl;
    std::cout << "x:" << node->x << ",y:" << node->y << std::endl;
    while (node != nullptr) {
      best_path.x.push_back(node->x);
      best_path.y.push_back(node->y);
      std::cout << "x:" << node->x << ",y:" << node->y << std::endl;
      node = node->precent_node;
    }
    std::reverse(best_path.x.begin(), best_path.x.end());
    std::reverse(best_path.y.begin(), best_path.y.end());

    // 平滑处理
    smoother();
  };

  // 对dp的计算结果进行三次样条平滑
  void smoother() {
    Spline2D csp_obj(best_path.x, best_path.y);
    Vec_f r_x;
    Vec_f r_y;
    Vec_f ryaw;
    Vec_f rcurvature;
    Vec_f rs;

    // 对平滑后的曲线进行加密
    float det_s = 0.1;
    for (float i = 0; i < csp_obj.s.back(); i += det_s) {
      std::array<float, 2> point_ = csp_obj.calc_postion(i);
      r_x.push_back(point_[0]);
      r_y.push_back(point_[1]);
      ryaw.push_back(csp_obj.calc_yaw(i));
      rcurvature.push_back(csp_obj.calc_curvature(i));
      rs.push_back(i);
    }
    best_path_smoother.x = std::move(r_x);
    best_path_smoother.y = std::move(r_y);
    best_path_smoother.heading = std::move(ryaw);
    best_path_smoother.kappa = std::move(rcurvature);
    best_path_smoother.s = std::move(rs);
  };

  void path_plot() {
    plt::figure();
    plt::named_plot("best path", best_path.x, best_path.y, "b-o");
    plt::named_plot("best path", best_path_smoother.x, best_path_smoother.y,
                    "r-o");
    plt::xlabel("x(m)");
    plt::ylabel("y(m)");
    plt::legend();
    plt::grid(true);
    plt::axis("equal");
    plt::show();
  }
};

int main() {
  std::cout << "set the resolution" << std::endl;
  DP dp_path(1.0, 0.5);
  dp_path.set_init_state(0.0, 3.5, 0.0);
  dp_path.generate_map(20, 3.5);
  std::cout << "generate map success" << std::endl;
  
  dp_path.get_final_path();
  dp_path.path_plot();
  return 0;
}
